// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
#include <serial_driver/serial_driver.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// C++ system
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <chrono>

#include "rm_serial_driver/crc.hpp"
#include "rm_serial_driver/packet.hpp"
#include "rm_serial_driver/rm_serial_driver.hpp"

namespace rm_serial_driver {
    RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions &options)
            : Node("rm_serial_driver", options),
              owned_ctx_{new IoContext(2)},
              serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)} {
        RCLCPP_INFO(get_logger(), "Start RMSerialDriver!");

        getParams();
        shoot_thres = 0;

        // TF broadcaster
        timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

        // Create Publisher
        latency_pub_ = this->create_publisher<std_msgs::msg::Float64>("/latency", 10);
        marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/aiming_point", 10);

        // Detect parameter client
        detector_param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "armor_detector");

        // Tracker reset service client
        reset_tracker_client_ = this->create_client<std_srvs::srv::Trigger>("/tracker/reset");

        try {
            serial_driver_->init_port(device_name_, *device_config_);
            if (!serial_driver_->port()->is_open()) {
                serial_driver_->port()->open();
                receive_thread_ = std::thread(&RMSerialDriver::receiveData, this);
            }
        } catch (const std::exception &ex) {
            RCLCPP_ERROR(
                    get_logger(), "Error creating serial port: %s - %s", device_name_.c_str(), ex.what());
            throw ex;
        }

        aiming_point_.header.frame_id = "odom";
        aiming_point_.ns = "aiming_point";
        aiming_point_.type = visualization_msgs::msg::Marker::SPHERE;
        aiming_point_.action = visualization_msgs::msg::Marker::ADD;
        aiming_point_.scale.x = aiming_point_.scale.y = aiming_point_.scale.z = 0.12;
        aiming_point_.color.r = 1.0;
        aiming_point_.color.g = 1.0;
        aiming_point_.color.b = 1.0;
        aiming_point_.color.a = 1.0;
        aiming_point_.lifetime = rclcpp::Duration::from_seconds(0.1);

        // Create Subscription
        target_sub_ = this->create_subscription<auto_aim_interfaces::msg::Target>(
                "/tracker/target", rclcpp::SensorDataQoS(),
                std::bind(&RMSerialDriver::sendData, this, std::placeholders::_1));
                
    //     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
    //                    "cmd_vel_chassis", 1, 
    //                    std::bind(&RMSerialDriver::cmdVelCallBack, this, std::placeholders::_1));
    // 
    }

    RMSerialDriver::~RMSerialDriver() {
        if (receive_thread_.joinable()) {
            receive_thread_.join();
        }

        if (serial_driver_->port()->is_open()) {
            serial_driver_->port()->close();
        }

        if (owned_ctx_) {
            owned_ctx_->waitForExit();
        }
    }

    void RMSerialDriver::receiveData() {
        std::vector <uint8_t> header(1);
        std::vector <uint8_t> data;
        data.reserve(sizeof(ReceivePacket));

        while (rclcpp::ok()) {
            try {
                serial_driver_->port()->receive(header);

                if (header[0] == 0x5A) {
                    data.resize(sizeof(ReceivePacket) - 1);
                    serial_driver_->port()->receive(data);

                    data.insert(data.begin(), header[0]);
                    ReceivePacket packet = fromVector(data);
                    //RCLCPP_INFO(get_logger(),"pitch:%f,yaw:%f,roll:%f,reset:%d,color:%d",packet.pitch,packet.yaw,packet.roll,packet.reset_tracker,packet.detect_color);
                    // bool crc_ok =
                    //         crc16::Verify_CRC16_Check_Sum(reinterpret_cast<const uint8_t *>(&packet), sizeof(packet));
                    if (1) {
                        if (!initial_set_param_ || packet.detect_color != previous_receive_color_) {
                            //receive data->color != 
                            setParam(rclcpp::Parameter("detect_color", packet.detect_color));
                            previous_receive_color_ = packet.detect_color;
                        }

                        if (packet.reset_tracker) {
                            resetTracker();
                        }
                        //组织坐标变换消息
                        // geometry_msgs::msg::TransformStamped t;
                        // timestamp_offset_ = this->get_parameter("timestamp_offset").as_double();
                        // t.header.stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
                        // t.header.frame_id = "gimbal_link";
                        // t.child_frame_id = "camera_link";
                        // t.transform.translation.x=0;
                        // t.transform.translation.y=0;
                        // t.transform.translation.z=0.211111;
                        // tf2::Quaternion q;
                        // //设置四元数
                        // q.setRPY(packet.roll/180*3.1415, -1*packet.pitch/180*3.1415, packet.yaw/180*3.1415);
                        // t.transform.rotation = tf2::toMsg(q);
                        //tf_broadcaster_->sendTransform(t);

                        if (abs(packet.aim_x) > 0.01) {
                            aiming_point_.header.stamp = this->now();
                            aiming_point_.pose.position.x = packet.aim_x;
                            aiming_point_.pose.position.y = packet.aim_y;
                            aiming_point_.pose.position.z = packet.aim_z;
                            marker_pub_->publish(aiming_point_);
                        }
                    } else {
                        //RCLCPP_ERROR(get_logger(), "CRC error!");
                    }
                } else {
                    //RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 20, "Invalid header: %02X", header[0]);
                }
            } catch (const std::exception &ex) {
                RCLCPP_ERROR_THROTTLE(
                        get_logger(), *get_clock(), 20, "Error while receiving data: %s", ex.what());
                reopenPort();
            }
        }
    }

    void RMSerialDriver::sendData(const auto_aim_interfaces::msg::Target::SharedPtr msg) {
        // 定义静态的id与uint8_t的映射关系表
        const static std::map <std::string, uint8_t> id_unit8_map{
                {"",        0},
                {"outpost", 0},
                {"1",       1},
                {"1",       1},
                {"2",       2},
                {"3",       3},
                {"4",       4},
                {"5",       5},
                {"guard",   6},
                {"base",    7}};

        try {
            // 构造发送包
            SendPacket packet;
            packet.tracking = msg->tracking;
            packet.id = id_unit8_map.at(msg->id);
            packet.armors_num = msg->armors_num;
            double x = msg->position.x;
            double offset = 0;
            double max_vyaw = 0.0;
            if(msg->tracking){
                offset = 0.00357841*x*x*x*x - 0.06*x*x*x +0.36030835*x*x - 0.88691018*x +0.76211092;
            } else {
                max_vyaw = 0.0;
            }
            RCLCPP_INFO(get_logger(),"offset = %lf",offset);
            packet.pitch = (msg->pitch +offset -0.04)*1000;
            last_pitch = packet.pitch/1000;
            double v_yaw = msg->v_yaw ;
            packet.v_yaw = msg->v_yaw;
            //double t  = 0.05;
            //packet.yaw = (msg->yaw+0.013)*1000;
            if (abs(shoot_thres - max_thres/2)<2){
                packet.yaw = msg->yaw*1000;
                packet.pitch = (msg->pitch +offset -0.01)*1000;
            }
            RCLCPP_INFO(get_logger(),"send_yaw =  %d",packet.yaw);
            //packet.yaw = 0;
            //dst = 3m shoot_thres >= max_thres/3
            if (shoot_thres >= max_thres*0.1 ){
                if (abs(v_yaw) > max_vyaw){
                    if(v_yaw > 0){
                        max_vyaw = -1 *v_yaw;
                    } else {
                        max_vyaw = v_yaw;
                    }
                }  
                //packet.yaw = (msg->yaw + 0.013 + max_vyaw*t)*1000;
                shoot_thres ++;
                packet.v_yaw = abs(max_vyaw)*1000;
                packet.shoot_mode = 1;
                if (shoot_thres>=max_thres){
                    max_thres= shoot_thres;
                }
                RCLCPP_INFO(get_logger(),"starting antitop");
                RCLCPP_INFO(get_logger(),"max_vyaw =  %lf",max_vyaw);
                RCLCPP_INFO(get_logger(),"max_thres = %d",max_thres);
            } else if (shoot_thres < max_thres*0.1 && abs(v_yaw) > 0.5){
                if (abs(v_yaw) > max_vyaw){
                    max_vyaw = abs(v_yaw);
                }
                shoot_thres ++;
            } else if (abs(v_yaw) < 0.5 && shoot_thres>=0){
                shoot_thres --;
                
            } else if ( shoot_thres<0){
                shoot_thres =0;
                packet.shoot_mode = 0;
            } else {
                shoot_thres = 0;
            }
            if (msg->tracking ==0 ){
                
                packet.yaw = 0.013;
                shoot_thres = 0;
                packet.shoot_mode = 0;
            }
            RCLCPP_WARN(get_logger(),"shoot_thres = %d  mode = %d",shoot_thres,packet.shoot_mode);
            
            // 计算并添加CRC16校验码
            crc16::Append_CRC16_Check_Sum(reinterpret_cast<uint8_t *>(&packet), sizeof(packet));
            RCLCPP_INFO(get_logger()," %d %d pitch = %lf yaw = %lf dst = %lf v_yaw = %lf",packet.id,packet.armors_num,msg->pitch,msg->yaw,msg->position.x ,msg->v_yaw);
            printf("%d",msg->tracking);
            // 将发送包转换为字节流
            std::vector <uint8_t> data = toVector(packet);
            // 将字节流发送到串口
            serial_driver_->port()->send(data);
            // 计算并发布延迟时间
            std_msgs::msg::Float64 latency;
            latency.data = (this->now() - msg->header.stamp).seconds() * 1000.0;
            RCLCPP_DEBUG_STREAM(get_logger(), "Total latency: " + std::to_string(latency.data) + "ms");
            latency_pub_->publish(latency);
        } catch (const std::exception &ex) {
            // 发生异常时记录错误日志并重新打开串口
            RCLCPP_ERROR(get_logger(), "Error while sending data: %s", ex.what());
            reopenPort();
        }
    }
    
    // void RMSerialDriver::cmdVelCallBack(const geometry_msgs::msg::Twist::SharedPtr msg){
    //   try {
    //     SendPacket packet;
    //         packet.tracking = 0;
    //         packet.id = 0;
    //         packet.armors_num = 0;
    //         packet.pitch = 0;
    //         packet.yaw = 0;
    //         packet.speed = 15*1000;
    //         packet.vx = msg->linear.x*5000;
    //         packet.vy = msg->linear.y*5000;
    //         // 计算并添加CRC16校验码
    //         crc16::Append_CRC16_Check_Sum(reinterpret_cast<uint8_t *>(&packet), sizeof(packet));
    //         RCLCPP_INFO(get_logger(),"%f %f",msg->linear.x,msg->linear.y);
    //         // 将发送包转换为字节流
    //         std::vector <uint8_t> data = toVector(packet);
    //         // 将字节流发送到串口
    //         serial_driver_->port()->send(data);
        
    //   } catch (tf2::TransformException & ex) {
    //     RCLCPP_ERROR(get_logger(), "Error while sending data: %s", ex.what());
    //         reopenPort();
    //   }
    // }
    
    void RMSerialDriver::getParams() {
        using FlowControl = drivers::serial_driver::FlowControl;
        using Parity = drivers::serial_driver::Parity;
        using StopBits = drivers::serial_driver::StopBits;

        uint32_t baud_rate{};
        auto fc = FlowControl::NONE;
        auto pt = Parity::NONE;
        auto sb = StopBits::ONE;

        try {
            device_name_ = declare_parameter<std::string>("device_name", "");
        } catch (rclcpp::ParameterTypeException &ex) {
            RCLCPP_ERROR(get_logger(), "The device name provided was invalid");
            throw ex;
        }

        try {
            baud_rate = declare_parameter<int>("baud_rate", 0);
        } catch (rclcpp::ParameterTypeException &ex) {
            RCLCPP_ERROR(get_logger(), "The baud_rate provided was invalid");
            throw ex;
        }

        try {
            const auto fc_string = declare_parameter<std::string>("flow_control", "");

            if (fc_string == "none") {
                fc = FlowControl::NONE;
            } else if (fc_string == "hardware") {
                fc = FlowControl::HARDWARE;
            } else if (fc_string == "software") {
                fc = FlowControl::SOFTWARE;
            } else {
                throw std::invalid_argument{
                        "The flow_control parameter must be one of: none, software, or hardware."};
            }
        } catch (rclcpp::ParameterTypeException &ex) {
            RCLCPP_ERROR(get_logger(), "The flow_control provided was invalid");
            throw ex;
        }

        try {
            const auto pt_string = declare_parameter<std::string>("parity", "");

            if (pt_string == "none") {
                pt = Parity::NONE;
            } else if (pt_string == "odd") {
                pt = Parity::ODD;
            } else if (pt_string == "even") {
                pt = Parity::EVEN;
            } else {
                throw std::invalid_argument{"The parity parameter must be one of: none, odd, or even."};
            }
        } catch (rclcpp::ParameterTypeException &ex) {
            RCLCPP_ERROR(get_logger(), "The parity provided was invalid");
            throw ex;
        }

        try {
            const auto sb_string = declare_parameter<std::string>("stop_bits", "");

            if (sb_string == "1" || sb_string == "1.0") {
                sb = StopBits::ONE;
            } else if (sb_string == "1.5") {
                sb = StopBits::ONE_POINT_FIVE;
            } else if (sb_string == "2" || sb_string == "2.0") {
                sb = StopBits::TWO;
            } else {
                throw std::invalid_argument{"The stop_bits parameter must be one of: 1, 1.5, or 2."};
            }
        } catch (rclcpp::ParameterTypeException &ex) {
            RCLCPP_ERROR(get_logger(), "The stop_bits provided was invalid");
            throw ex;
        }

        device_config_ =
                std::make_unique<drivers::serial_driver::SerialPortConfig>(baud_rate, fc, pt, sb);
    }

    void RMSerialDriver::reopenPort() {
        RCLCPP_WARN(get_logger(), "Attempting to reopen port");
        try { 
            if (serial_driver_->port()->is_open()) {
                serial_driver_->port()->close();
            }
            serial_driver_->port()->open();
            RCLCPP_INFO(get_logger(), "Successfully reopened port");
        } catch (const std::exception &ex) {
            RCLCPP_ERROR(get_logger(), "Error while reopening port: %s", ex.what());
            if (rclcpp::ok()) {
                rclcpp::sleep_for(std::chrono::seconds(1));
                reopenPort();
            }
        }
    }

    void RMSerialDriver::setParam(const rclcpp::Parameter &param) {
        if (!detector_param_client_->service_is_ready()) {
            RCLCPP_WARN(get_logger(), "Service not ready, skipping parameter set");
            return;
        }

        if (
                !set_param_future_.valid() ||
                set_param_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
            RCLCPP_INFO(get_logger(), "Setting detect_color to %ld...", param.as_int());
            set_param_future_ = detector_param_client_->set_parameters(
                    {param}, [this, param](const ResultFuturePtr &results) {
                        for (const auto &result: results.get()) {
                            if (!result.successful) {
                                RCLCPP_ERROR(get_logger(), "Failed to set parameter: %s", result.reason.c_str());
                                return;
                            }
                        }
                        RCLCPP_INFO(get_logger(), "Successfully set detect_color to %ld!", param.as_int());
                        initial_set_param_ = true;
                    });
        }
    }

    void RMSerialDriver::resetTracker() {
        if (!reset_tracker_client_->service_is_ready()) {
            RCLCPP_WARN(get_logger(), "Service not ready, skipping tracker reset");
            return;
        }

        auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
        reset_tracker_client_->async_send_request(request);
        RCLCPP_INFO(get_logger(), "Reset tracker!");
    }

}  // namespace rm_serial_driver

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rm_serial_driver::RMSerialDriver)
